clear all
% close all

springN		= 1;
run_ctl		= 1;
save_plots	= 0;

fprintf('\nStiffness script START\n');
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

str = sprintf('./spring%d/sys%d.mat',springN,springN);
load(str);
str = sprintf('./spring%d/ctrl%d.mat',springN,springN);
load(str);

log_name = sprintf('./fixedtoe/k%d_logs/test',springN);

tauc	= sys.tauc;
GR		= sys.GR;

K		= sys.K;
B		= sys.B;
I		= sys.I;
OFFSET	= sys.OFFSET;


w0	= sqrt(K/I);
z	= B/(2*sqrt(K*I));
wd	= w0*sqrt(1-z^2);
fd	= wd/(2*pi);

A2T = tauc*GR;

n = 100;

t_start = 6;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Run controllers

f = logspace(0,2,n);

% idx1 = find(f>fd-6,1);
% idx2 = find(f<fd+6,1,'last');
% f = [f(1:idx1) f(idx2:end)];
% n = length(f);

w		= 2*pi*f;

a		= ctrl.a;
b		= ctrl.b;
k1		= ctrl.k1;
k2		= ctrl.k2;
ke1		= ctrl.ke1;
ke2		= ctrl.ke2;
trq_lim = ctrl.trq_lim;


if (run_ctl == 1)
for test_idx = 1:n

	t_end	= t_start+4*1/f(test_idx);

	fprintf('Running controller:\n');
	fprintf('Command Frequency: %f\n',f(test_idx));
	%			controller	log_file	time_start	time_stop	[controller_parms]
	cmd_str = sprintf('sudo ./bin/fca_interface_headless 4 %s.%d.log %.6f %.6f %.6f %.6f %.6f %d %d %d %d %.6f > /dev/null', log_name,test_idx, t_start, t_end, a, w(test_idx), b, k1, k2, ke1, ke2, trq_lim);
	
% 	cmd_str = sprintf(6'sudo ./bin/fca_interface_headless 6 %s.%d.log %.6f %.6f %.6f %.6f %.6f %d %d %d %d %.6f > /dev/null', log_name,test_idx, t_start, t_end, sys.K, sys.B, sys.I, f(test_idx), a, trq_lim);
	
	fprintf('%s\n\n',cmd_str);
	
	ret = unix(cmd_str);
	if (ret ~= 0)
		fprintf('Controller fail!\n');
		return;
	end
	
	
	pause(0.5);

end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compile logs

fprintf('Compiling logs\n');
f_lim = 0;


for test_idx = 1:n
	fprintf('Opening log:\t');
	fprintf('%s.%d.log\n',log_name,test_idx);
	
	log_str = sprintf('%s.%d.log',log_name,test_idx);
	log = importdata(log_str);
	
% 	motor_angle(:,test_idx)		= log.data(:,4);
% 	leg_angle(:,test_idx)		= log.data(:,5);
% 	
% 	motor_current(:,test_idx)	= log.data(:,8);
	
%%%%%%
	motor_angle		= log.data(:,4);
	leg_angle		= log.data(:,5);
	
	motor_current	= log.data(:,8);
	
	spring_pos = motor_angle - leg_angle + OFFSET;

	spring_trq = K*spring_pos;
	
	t = log.data(:,1);
	
	W = ceil(length(t)*0.03);
	if (W>10)
		W = 10;
	end
	
% 	idx = round(500/f(test_idx));
	idx = length(t);

	
	mtr_trq = motor_current*A2T;
	
	temporary	= filter( repmat( 1/2/W, 1, W ), 1, mtr_trq( end : -1 : 1, : ) );
	filt_mtr	= filter( repmat( 1/2/W, 1, W ), 1, mtr_trq ) ...
				+  temporary( end : -1 : 1, : );
			
	temporary	= filter( repmat( 1/2/W, 1, W ), 1, spring_trq( end : -1 : 1, : ) );
	filt_s		= filter( repmat( 1/2/W, 1, W ), 1, spring_trq ) ...
				+  temporary( end : -1 : 1, : );
	

	mtr_trq		= filt_mtr;
%  	spring_trq	= filt_s;

	
	amps(test_idx) = max(abs(spring_trq));
	
	phase(test_idx) = 0;
 	
	if (f_lim == 0)
% 		if (sum(abs(mtr_trq)>=trq_lim) >= ceil(length(t)*0.01))
		if (max(abs(mtr_trq(1:idx)))>=trq_lim)
			f_lim = f(test_idx)
% 			figure
% 			hold on
% 			plot([t(1) t(end)],[trq_lim trq_lim],'k--');
% 			plot([t(1) t(end)],[-trq_lim -trq_lim],'k--');
% 			plot(t,mtr_trq);
% 			plot(t,spring_trq,'r');
		end
	end
	
% 	if (f(test_idx) >= 25) && (f(test_idx) <= 26)
% 		figure
% 		hold on
% 		plot([t(1) t(end)],[trq_lim trq_lim],'k--');
% 		plot([t(1) t(end)],[-trq_lim -trq_lim],'k--');
% 		plot(t,mtr_trq);
% 		plot(t,spring_trq,'r');
% 	end
	
end


% t = log.data(:,1);
% 
% mtr_trq = motor_current*A2T;
% 
% spring_pos = motor_angle - leg_angle + OFFSET;
% 
% spring_trq = K*spring_pos;
% 
% % window size: TODO make function of time
% W = length(t)*0.01;
% 
% temporary = filter( repmat( 1/2/W, 1, W ), 1, spring_trq( end : -1 : 1, : ) );
% filtered = filter( repmat( 1/2/W, 1, W ), 1, spring_trq ) ...
% 	+  temporary( end : -1 : 1, : );
% figure;
% plot( t, [spring_trq(:,1) filtered( :, 1 )], 'linewidth', 3 );
% 
% amps = max(abs(filtered));
% f_lim = f(find(max(mtr_trq) < trq_lim,1,'last'));

mag = 20*log10(amps/a);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
h = figure;
set(h,'position',[ 100 100 400 400])

semilogx([f(1) f(end)], [0 0],'k--','linewidth', 2);
hold on

semilogx([f(1) f(end)], [3 3],'k--','linewidth', 1);
semilogx([f(1) f(end)], [-3 -3],'k--','linewidth', 1);

semilogx([fd fd], [-3 3],'g','linewidth', 2);
text(fd,max(amps)- max(amps)*0.05,'f_d \rightarrow','FontSize',12, 'HorizontalAlignment', 'right');

if (f_lim ~= 0)
	semilogx([f_lim f_lim], [-3 3],'r','linewidth', 2);
	text(f_lim,max(amps)- max(amps)*0.1,'\leftarrow \tau_{lim}','FontSize',12, 'HorizontalAlignment', 'left');
end
% plot(f,amps,'linewidth', 2);
semilogx(f,mag,'linewidth', 2);
axis tight

xlabel(	'\textbf{Frequency} $\left( H z \hspace{1 mm}\right)$',...
		'interpreter','latex','fontsize',16);

ylabel(	'\textbf{Magnitude} (dB )',...
		'interpreter','latex','fontsize',16);

set(gca,'Position',[ 0.15 0.15 0.8 0.8])
	
str = sprintf('/home/drl/projects/jhurst/current/Kemper_journal/images/bode%d.pdf', springN);
if (save_plots == 1)
	save2pdf(str);
end


% figure
% hold on
% plot([t(1) t(end)], [a a], 'k','linewidth', 2);
% plot([t(1) t(end)], [-a -a], 'k','linewidth', 2);
% plot(t,spring_trq,'linewidth', 2);
% axis tight


% figure
% hold on
% plot(t,mtr_trq,'linewidth', 2);
% axis tight


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% mail = 'kkemper@gmail.com'; %Your GMail email address
% password = '57d.alN.'; %Your GMail password
% % Don't touch unless you need to change the Email supplier (currently Gmail)
% setpref('Internet','E_mail',mail);
% setpref('Internet','SMTP_Server','smtp.gmail.com');
% setpref('Internet','SMTP_Username',mail);
% setpref('Internet','SMTP_Password',password);
% props = java.lang.System.getProperties;
% props.setProperty('mail.smtp.auth','true');
% props.setProperty('mail.smtp.socketFactory.class', 'javax.net.ssl.SSLSocketFactory');
% props.setProperty('mail.smtp.socketFactory.port','465');
% 
% sendmail('kkemper@gmail.com','matlab is done!')